Planning of the motion of a manipulator is generally performed in two steps: trajectory planning and motion planning. Collision-free trajectory planning is typically solved by decomposing the problem into two steps. First, only the geometric aspects of the manipulator, including the collision avoidance constraint, are considered. Next, the planning accounts for other constraints, such as the constraints on system dynamics. After the trajectory is determined, a speed profile can be assigned to the trajectory to determine the motion of the manipulator. Such decomposition can be computationally efficient. However, that approach can fail to find a feasible trajectory when the geometric path is not properly generated. Also, this approach does not ensure an optimal, trajectory.
The planning of an optimal trajectory of a manipulator avoiding obstacles while minimizing energy consumption of the manipulator in the field of robotics is difficult. This is because the requirement of avoiding obstacles leads to a set of non-convex constraints and the optimization of continuous-time dynamical systems for the set of non-convex constraint is a difficult problem to solve.
Several conventional methods optimize energy efficiency during the path planning step by associating geometry, e.g., curvature of the trajectory with energy consumption. Some methods consider the energy-efficient path planning problem in terms of the friction and gravity, but assume the manipulator moves at a constant speed. Results on the energy optimal motion planning are limited, and some methods focus on time-optimality and smoothness of the trajectory, see, e.g., U.S. Pat. No. 6,216,058.
The non-convex constraints due to obstacle avoidance need to be enforced during trajectory generation and optimization. Several conventional methods formulate the trajectory planning with obstacle avoidance as an optimization problem. For example, the method described in U.S. Pat. No. 7,248,952 formulates the trajectory planning as a mixed integer linear programming problem. However, the computation of a mixed integer problem is time consuming. Another method, described by Hagenaars et al, in “Approximate continuous-time optimal control in obstacle avoidance by time/space discretization of non-convex state constraints,” Proc. of the 2004 IEEE Int. Conf. on Control Applications pp. 878-883, 2004, considers optimal control with obstacles by discretization of time and space. However, the cost function of the state and control variables is convex.
Some methods determine a suboptimal trajectory planning by minimizing influence of the non-convex constraints. For example, a method described by Duleha et al., in “Nonholonomic motion planning based on newton algorithm with energy optimization,” IEEE Transactions on Control System Technology, Vol: 11(3), pp. 355-363, May 2003, considers energy efficient trajectory planning for non-holonomic systems without obstacles. Yet another method described by Mei et al., in “Energy-efficient motion planning for mobile robots,” In Proc. of IEEE Int. Cone on Robotics and Automation, pp. 4344-4349, 2004, considers the energy efficient motion planning for a mobile robot which has to visit several locations. The robot is assumed to move at certain pattern, thus simplifying the energy optimal trajectory planning problem by ignoring the dynamics of robots. However, all those methods do not take obstacles into account.
Accordingly, there is a need for optimization of a trajectory of a motion of a manipulator avoiding at least one obstacle, wherein the optimization is subject to spatial non-convex constraints due to the obstacle.